#include "absl/memory/memory.h"
#include "cartographer/mapping/map_builder.h"
#include "cartographer_ros/node.h"
#include "cartographer_ros/node_options.h"
#include "cartographer_ros/ros_log_sink.h"
#include "gflags/gflags.h"
#include "tf2_ros/transform_listener.h"

DEFINE_bool(collect_metrics, false,
            "Activates the collection of runtime metrics. If activated, the "
            "metrics can be accessed via a ROS service.");

DEFINE_double(delay_time, 0.0, "");

DEFINE_string(configuration_directory, "",
              "First directory in which configuration files are searched, "
              "second is always the Cartographer installation to allow "
              "including files from there.");

DEFINE_string(configuration_basename, "",
              "Basename, i.e. not containing any directory prefix, of the "
              "configuration file.");
DEFINE_string(load_state_filename, "",
              "If non-empty, filename of a .pbstream file to load, containing "
              "a saved SLAM state.");

DEFINE_bool(load_frozen_state, true, "Load the saved state as frozen (non-optimized) trajectories.");
DEFINE_bool(start_trajectory_with_default_topics, true, "Enable to immediately start the first trajectory with default topics.");
DEFINE_string(save_state_filename, "", "If non-empty, serialize state and write it to disk before shutting down.");

namespace cartographer_ros
{
    namespace
    {
        void Run()
        {
            rclcpp::Node::SharedPtr cartographer_node = rclcpp::Node::make_shared("cartographer_node");
            constexpr double kTfBufferCacheTimeInSeconds = 10.;

            std::shared_ptr<tf2_ros::Buffer> tf_buffer = std::make_shared<tf2_ros::Buffer>(
                cartographer_node->get_clock(), tf2::durationFromSec(kTfBufferCacheTimeInSeconds), cartographer_node);

            std::shared_ptr<tf2_ros::TransformListener> tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);

            NodeOptions node_options;
            TrajectoryOptions trajectory_options;
            std::tie(node_options, trajectory_options) = LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

            auto map_builder = cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
            auto node = std::make_shared<cartographer_ros::Node>(node_options, std::move(map_builder), tf_buffer, cartographer_node,
                                                                 FLAGS_collect_metrics);
            if (!FLAGS_load_state_filename.empty())
            {
                node->LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
            }
            node->delay_time = FLAGS_delay_time;
            LOG(INFO) << "delay_time " << FLAGS_delay_time;
            if (FLAGS_start_trajectory_with_default_topics)
            {
                node->StartTrajectoryWithDefaultTopics(trajectory_options);
            }
            rclcpp::spin(cartographer_node);
            node->FinishAllTrajectories();
            node->RunFinalOptimization();
            if (!FLAGS_save_state_filename.empty())
            {
                // true indicates including unfinished submaps
                node->SerializeState(FLAGS_save_state_filename, true);
            }
        }
    } // namespace
} // namespace cartographer_ros

int main(int argc, char **argv)
{
    // Init rclcpp first because gflags reorders command line flags in argv
    rclcpp::init(argc, argv);
    google::AllowCommandLineReparsing();
    google::InitGoogleLogging(argv[0]);
    google::ParseCommandLineFlags(&argc, &argv, false);
    CHECK(!FLAGS_configuration_directory.empty()) << "-configuration_directory is missing.";
    CHECK(!FLAGS_configuration_basename.empty()) << "-configuration_basename is missing.";
    cartographer_ros::ScopedRosLogSink ros_log_sink;
    cartographer_ros::Run();
    ::rclcpp::shutdown();
}
